//#include <vector>
#include <string>
#include "datastructure.h"

using namespace std;

class PCloud
{
private:
	vector<dPoint> pc_points;
	unsigned int pc_dim;		
	//dPoint pc_pmin;
	//dPoint pc_pmax;
	//char pc_name[256];

public:
	vector<vector<int>> faces; // for viewing

	//Constructor
	PCloud():pc_dim(3) {}
	
	// _points=[x1 x2 ... xn y1 y2 ... yn z1 z2 ... zn]. Read all points[xi yi zi] from vector "_points" and add them into PCloud. np is the number of points
	PCloud(double* _points, unsigned int _np, unsigned int _dim)
	{
		vector<double> coords;
		coords.resize(_dim);
		for (unsigned int i = 0; i < _np; i ++)
		{
			for (unsigned int j = 0; j < _dim; j ++)
			{
				coords[j] = _points[j * _np + i];
			}
			add_point(dPoint(_dim, coords.begin(), coords.end()));
		}
	}

	//Destructor
	virtual ~PCloud() {};

	//Functions for Creation: read points from .m file
	bool read_from_file(string _filename)
	{
		pc_points.clear();
		faces.clear();

		FILE* fileModel = fopen(_filename.c_str(), "r");

		if (fileModel == NULL)
		{
			std::cout << "Error! Cannot open file. Check the directory: " << _filename.c_str() << std::endl;

			char* path;
			_get_pgmptr(&path);
			printf(path);

			return false;
		}
		else
		{
			char typeUnit;

			while ((typeUnit = fgetc(fileModel)) != EOF)
			{
				if (typeUnit == 'V')
				{
					int idV;
					float x, y, z;

					bool rd = fscanf(fileModel, "ertex %i %f %f %f\n", &idV, &x, &y, &z);

					if (rd)
					{
						std::vector<float> temp_coord;
						//temp_coord.resize(pc_dim);
						temp_coord.push_back(x);
						temp_coord.push_back(y);
						temp_coord.push_back(z);

						//std::cout << "x= " << x << "; y= " << y << ";z= " << z << std::endl; 
						dPoint dp = dPoint(pc_dim, temp_coord.begin(), temp_coord.end());
						//std::cout << "dp.x= " << dp.hx() << std::endl;
						//add_point(dp);
						pc_points.push_back(dp);	
						//list_points.push_back(temp_point);
					}
				}

				if (typeUnit == 'F')
				{
					int idF;
					int p1, p2, p3;

					bool rd = fscanf(fileModel, "ace %i %i %i %i\n", &idF, &p1, &p2, &p3);

					if (rd)
					{
						std::vector<int> pts;						
						pts.push_back(p1);
						pts.push_back(p2);
						pts.push_back(p3);

						faces.push_back(pts);
					}
				}
			}

			std::cout << "File " << _filename << " was read successfully!" << std::endl;
			std::cout << "Number of points : " << pc_points.size() << std::endl;
			std::cout << "Number of faces : " << faces.size() << "\n" << std::endl;

			return true;
		}
	}

	void view_points()
	{
		for (int i=0; i< pc_points.size(); i++)
		{
			std::cout << "i= " << i+1 << ": x= " << pc_points[i].hx() << "; y= " << pc_points[i].hy() << "; z= " << pc_points[i].hz() << std::endl;
		}
	}

	//Function for accessing data
	unsigned int get_dim() const
	{
		return pc_dim;
	}

	void set_dim(unsigned int _dim)
	{
		pc_dim = _dim;
	}

	unsigned int get_cloud_size() const
	{
		return pc_points.size();
	}

	void add_point(const dPoint& _v)
	{
		//std::cout << ": x= " << _v.hx() << "; y= " << _v.hy() << "; z= " << _v.hz() << std::endl;
		pc_points.push_back(_v);	
	}

	dPoint& get_point(unsigned int _i)
	{			
		//std::cout << "i= " << _i << ": x= " << pc_vertices[_i].get_coord().hx() << "; y= " << pc_vertices[_i].get_coord().hy() << "; z= " << pc_vertices[_i].get_coord().hz() << std::endl;
		return pc_points[_i];
	}

	//double get_radius()
	//{
	//	return sqrt(CGAL::to_double((pc_pmax - pc_pmin) * (pc_pmax - pc_pmin))) / 2;
	//}

	//Average neighborhood size
	double average_size(unsigned int k)
	{
		typedef CGAL::Search_traits_d<KCd> Traits;
		typedef CGAL::Euclidean_distance<Traits> Distance;
		typedef CGAL::Fair<Traits> Fair;
		typedef CGAL::Orthogonal_k_neighbor_search<Traits,Distance,Fair> Neighbor_search;
		typedef Neighbor_search::Tree Tree;
		typedef CGAL::Fair<Traits> Fair;
		typedef KCd::FT FT;

		unsigned int np = get_cloud_size();

		vector<dPoint> points;
		for(unsigned int i = 0; i < np; i ++)
		{
			points.push_back(get_point(i));		
		}

		Fair fair(10);
		Tree tree(points.begin(), points.end(), fair);

		double eps = 1e-3;	//approximation factor eps
		double as = 0;
		for(unsigned int i = 0; i < np; i ++)
		{
			dPoint pt = get_point(i);
			Neighbor_search search(tree, pt, k, FT(eps)); //// Find k neighbors

			FT dist(0);
			unsigned int nfound = 0;
			for(Neighbor_search::iterator iter = search.begin(); iter != search.end(); iter ++)
			{
				dist += sqrt( iter->second);
				nfound ++;
			}

			if(nfound > 0)
			{
				as += (dist / nfound);
			}

			//printf("Number of neighbors: %i\n", nfound);
		}
		return as / np;
	}
		
	//Clear
	void clear()
	{
		pc_points.clear();
	}
};

